﻿#region License Info
//Component of Cronos Package, http://www.codeplex.com/cronos
//Copyright (C) 2009 Anthony Brockwell

//This program is free software; you can redistribute it and/or
//modify it under the terms of the GNU General Public License
//as published by the Free Software Foundation; either version 2
//of the License, or (at your option) any later version.

//This program is distributed in the hope that it will be useful,
//but WITHOUT ANY WARRANTY; without even the implied warranty of
//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//GNU General Public License for more details.

//You should have received a copy of the GNU General Public License
//along with this program; if not, write to the Free Software
//Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
#endregion


using System;
using System.Collections.Generic;
using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;

namespace ExtraControlLibrary
{
    public class SurfaceEstimate
    {
        private readonly long maxPartition;
        private readonly int partitions;
        private readonly List<double[]>[] pointContainers; // a point is an array of {arg_x,arg_y,...,value}
        private int numPoints;


        public SurfaceEstimate(Vector lower, Vector upper, int partitionsPerDimension)
        {
            LowerBound = lower;
            UpperBound = upper;
            Dimension = lower.Length;
            if (Dimension != upper.Length)
                throw new ArgumentException("Arguments have different dimensions.");
            partitions = partitionsPerDimension;
            maxPartition = 1;
            for (int i = 0; i < Dimension; ++i)
                maxPartition *= partitions;
            pointContainers = new List<double[]>[maxPartition];
            for (long i = 0; i < maxPartition; ++i)
                pointContainers[i] = new List<double[]>();
            numPoints = 0;
        }

        public int Dimension { get; private set; }

        public Vector LowerBound { get; private set; }
        public Vector UpperBound { get; private set; }

        public double MaxValue { get; private set; }
        public double MinValue { get; private set; }


        /// <summary>
        /// converts coordinate into partition number from 0 to maxPartition-1
        /// </summary>
        /// <param name="arg"></param>
        /// <returns></returns>
        private long PartitionNumber(Vector arg)
        {
            long digits = 1;
            long result = 0;
            for (int d = 0; d < Dimension; ++d)
            {
                double tx = (arg[d] - LowerBound[d])/(UpperBound[d] - LowerBound[d]);
                var idx = (int) Math.Floor(tx*partitions);
                if (idx >= partitions)
                    idx = partitions - 1;

                result += digits*idx;
                digits *= partitions;
            }
            return result;
        }

        public void MultiplyBy(double scale)
        {
            for (long i = 0 ; i<maxPartition ; ++i)
            {
                foreach (var x in pointContainers[i])
                   x[Dimension] *= scale;
            }
        }

        private List<Vector> GetPartitionCorners(long partitionNum, out List<Vector> dimRanges)
        {
            dimRanges = new List<Vector>(2*Dimension);
            var divs = new long[Dimension];
            long residual = partitionNum;
            for (int d = 0; d < Dimension; ++d)
            {
                divs[d] = residual%partitions;
                residual /= partitions;
            }
            for (int d = 0; d < Dimension; ++d)
            {
                double x1 = divs[d]*1.0/partitions;
                double x2 = x1 + 1.0/partitions;
                var range = new Vector(new[]
                                            {
                                                x1*(UpperBound[d] - LowerBound[d]) + LowerBound[d],
                                                x2*(UpperBound[d] - LowerBound[d]) + LowerBound[d]
                                            });
                dimRanges.Add(range);
            }

            // now convert endpoints to corners
            var corners = new List<Vector>();
            for (uint binaryCounter = 0; binaryCounter < (1 << Dimension); ++binaryCounter )
            {
                Vector tv = new Vector(Dimension);
                uint ccopy = binaryCounter;
                for (int d=0 ; d<Dimension ; ++d)
                {
                    if (ccopy % 2 == 0)
                        tv[d] = dimRanges[d][0];
                    else
                        tv[d] = dimRanges[d][1];
                    ccopy /= 2;
                }
                corners.Add(tv);
            }

            return corners;
        }


        public void AddPoint(Vector location, double value, double weight)
        {
            long pnum = PartitionNumber(location);
            var thePoint = new double[Dimension + 2];
            for (int i = 0; i < Dimension; ++i)
                thePoint[i] = location[i];
            thePoint[Dimension] = value;
            thePoint[Dimension + 1] = weight;
            pointContainers[pnum].Add(thePoint);

            if (numPoints==0)
            {
                MaxValue = value;
                MinValue = value;
            }
            else
            {
                if (value > MaxValue)
                    MaxValue = value;
                if (value < MinValue)
                    MinValue = value;
            }

            ++numPoints;
        }

        // returns list of neighbours of bins in binList
        private Set<long> NeighbouringBinList(Set<long> binList)
        {
            var neighbours = new Set<long>();

            foreach (int startBin in binList)
            {
                // determine current position
                var divs = new long[Dimension];
                long residual = startBin;
                for (int d = 0; d < Dimension; ++d)
                {
                    divs[d] = residual%partitions;
                    residual /= partitions;
                }

                for (int direction = 0; direction < 2; ++direction)
                    for (int curDim = 0; curDim < Dimension; ++curDim)
                    {
                        {
                            // determine neighbouring bin as specified by offset
                            int nBin = startBin;
                            int digit = 1;
                            bool edge = false;
                            for (int d = 0; d < curDim; ++d)
                                digit *= partitions;

                            if (direction > 0)
                                if (divs[curDim] < partitions)
                                    nBin += digit;
                                else
                                    edge = true;
                            if (direction == 0)
                                if (divs[curDim] > 0)
                                    nBin -= digit;
                                else
                                    edge = true;

                            // add it to the set
                            if (!edge)
                                neighbours.AddDistinct(nBin);
                        }
                    }
            }
            return neighbours;
        }

        /// <summary>
        /// returns list of all bins whose distance is less than or equal to bandwidth
        /// </summary>
        /// <param name="location"></param>
        /// <param name="bandwidth"></param>
        /// <returns></returns>
        private Set<long> BinsInNeighbourhood(Vector location, double bandwidth)
        {
            long startBin = PartitionNumber(location);
            var start = new Set<long>();
            start.Add(startBin);

            // expand list of neighbours until the new ones that have been added are all > bandwidth away
            Set<long> neighbours = null;
            bool done = false;

            while (!done)
            {
                double minDist = 1e100;
                neighbours = NeighbouringBinList(start);
                foreach (long newBin in neighbours)
                    if (!start.Contains(newBin))
                    {
                        List<Vector> ranges;
                        List<Vector> corners = GetPartitionCorners(newBin, out ranges);
                        double tx = BinDistanceFrom(corners, ranges, location);
                        if (tx < minDist)
                            minDist = tx;
                    }
                if (minDist <= bandwidth)
                    foreach (long newBin in neighbours)
                        start.AddDistinct(newBin);
                else
                    done = true;
            }
            return start;
        }

        private double BinDistanceFrom(List<Vector> corners, List<Vector> bounds, Vector location)
        {
            bool contained = true;
            for (int d = 0; (d < Dimension && contained); ++d)
                if (location[d] < bounds[d][0] || location[d] > bounds[d][1])
                    contained = false;
            if (contained)
                return 0.0;

            // otherwise compute minimum distance
            double minDistance = 1e100;
            foreach (Vector pt in corners)
            {
                double tx = (pt - location).Norm();
                if (tx < minDistance)
                    minDistance = tx;
            }
            return minDistance;
        }

        private static double WeightForDistanceBandwidth(double dist, double band)
        {
            double u = 1.0 - dist/band;
            return u;
        }

        /// <summary>
        /// returns locally averaged value of point
        /// </summary>
        /// <param name="location"></param>
        /// <returns></returns>
        public double GetValueAt(Vector location)
        {
            double totalValue = 0;
            double totalWeight = 0;

            // determine bandwidth for smoothing
            double bandwidth = 4*Math.Pow(numPoints, -1.0/Dimension + 0.1);

            // find all points within bandwidth

            Set<long> neighbours = BinsInNeighbourhood(location, bandwidth);
            foreach (long i in neighbours)
            {
                List<Vector> ranges;
                List<Vector> cornerList = GetPartitionCorners(i, out ranges);
                double tx = BinDistanceFrom(cornerList, ranges, location);
                if (tx <= bandwidth)
                {
                    foreach (var pt in pointContainers[i])
                    {
                        // compute distance
                        double dist2 = 0;
                        for (int d = 0; d < Dimension; ++d)
                            dist2 += (pt[d] - location[d])*(pt[d] - location[d]);
                        dist2 = Math.Sqrt(dist2);
                        if (dist2 < bandwidth)
                        {
                            double wt = WeightForDistanceBandwidth(dist2, bandwidth)*pt[Dimension + 1];
                            totalWeight += wt;
                            totalValue += wt*pt[Dimension];
                        }
                    }
                }
            }

            if (totalWeight > 0)
                return (totalValue/totalWeight);
            return 0;
        }
    }
}